#include "ThreadCANETUDP.h"


extern "C"{
#include <stdio.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <stdlib.h>
#include <string.h>
}


ThreadCANETUDP::ThreadCANETUDP(const char* target_ip, const char* target_port, const char* local_port)
{
//    int size_timeval =  sizeof(struct timeval);
//    int size_frame = sizeof(CAN_FRAME);
//    int size_total = MAX_NUM_OF_CAN_ID*(sizeof(CAN_FRAME) + sizeof(uint32_t));
//    m_recv_frames = new CAN_FRAME[MAX_NUM_OF_CAN_ID];
//    m_recv_id = new uint32_t[MAX_NUM_OF_CAN_ID];
//    memset(m_recv_frames, 0, MAX_NUM_OF_CAN_ID*sizeof(CAN_FRAME));

    memcpy(m_target_ip, target_ip, strlen(target_ip));
    memcpy(m_target_port, target_port, strlen(target_port));


    m_sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    if(m_sockfd<0)
        perror("socket");

    struct sockaddr_in addr;
    addr.sin_family = AF_INET;
    addr.sin_port = htons(atoi(local_port));
    addr.sin_addr.s_addr = htonl(INADDR_ANY);

    int ret = bind(m_sockfd, (struct sockaddr*)&addr, sizeof(addr));
    if(ret==0)
        printf("bind on port %s success\n", local_port);
    else
        perror("bind");
}

ThreadCANETUDP::~ThreadCANETUDP()
{

}


void ThreadCANETUDP::can_send(CAN_FRAME* can_frame)
{
    static pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
    pthread_mutex_lock(&mutex);


    uint8_t send_buffer[13];
    memset(send_buffer, 0, sizeof(send_buffer));
    if(can_frame->IDE)
        send_buffer[0] |= 0x80;
    if(can_frame->RTR)
        send_buffer[0] |= 0x40;
    send_buffer[0] |= can_frame->dlc & 0x0F;
    send_buffer[1] = (can_frame->id >> 24) & 0xFF;
    send_buffer[2] = (can_frame->id >> 16) & 0xFF;
    send_buffer[3] = (can_frame->id >> 8 ) & 0xFF;
    send_buffer[4] = (can_frame->id      ) & 0xFF;
    for(int i=0; i<can_frame->dlc; i++)
    {
        send_buffer[5+i] = can_frame->data[i];
    }

    struct sockaddr_in target_addr;
    target_addr.sin_family = AF_INET;
    target_addr.sin_port = htons(atoi(m_target_port));
    target_addr.sin_addr.s_addr = inet_addr(m_target_ip);


    sendto(m_sockfd, send_buffer, 13, 0,
                               (struct sockaddr*)&target_addr, sizeof(target_addr));

    pthread_mutex_unlock(&mutex);
}




void ThreadCANETUDP::set_msg_handler_callback(CanMsgHandlerCallback callback, void* node)
{
    msg_handler_callback = callback;
    parent = node;
}


//线程运行函数
void ThreadCANETUDP::routine()
{
    int recv_data_size = 13*50;
    unsigned char recv_buffer[recv_data_size];
    CAN_FRAME recv_frame;





    while(1)
    {

        int bytes = recv(m_sockfd,recv_buffer,recv_data_size, 0);
        if(bytes>=13)
        {
            int frame_num = bytes/13;
            if(bytes%13!=0)
                frame_num--;
            for(int i=0; i<frame_num; i++)
            {
                unsigned char* frame_buffer = recv_buffer + 13*i;
                memset(&recv_frame, 0, sizeof(recv_frame));
                gettimeofday(&recv_frame.timestamp, nullptr);
                recv_frame.id = (frame_buffer[1] << 24) + (frame_buffer[2] << 16)
                              + (frame_buffer[3] << 8 ) + (frame_buffer[4]);
                recv_frame.dlc = frame_buffer[0] & 0x0F;
                memcpy(recv_frame.data, frame_buffer+5, recv_frame.dlc);
                if((frame_buffer[0] & 0x80) == 0x80)
                    recv_frame.IDE = 1;
                if((frame_buffer[0] & 0x40) == 0x40)
                    recv_frame.RTR = 1;
                recv_frame.new_msg = 1;

                if(msg_handler_callback)
                {
                    msg_handler_callback(&recv_frame, parent);
                }
            }

        }


    }
}

